#include <intrin.h> 
#include "Process.h"

using namespace cv;
using namespace std;


const double scale = 128.0; 
static void getCannyKernel(OutputArray _d, double alpha)
{
	int r = cvRound(alpha);
	int ksize = 2 * r + 1;

	_d.create(1, ksize, CV_16S, -1, true);

	Mat k = _d.getMat();

	vector<float> kerF(ksize, 0.0f);
	kerF[r] = 0.0f;
	double a2 = alpha * alpha / 3;
	float sum = 0.0f;
	for (int x = 1; x <= r; ++x)
	{
		float v = (float)(-x * std::exp(-x * x / (2 * a2)));
		sum += v;
		kerF[r + x] = v;
		kerF[r - x] = -v;
	}
	float scale = 128 / sum;
	for (int i = 0; i < ksize; ++i)
	{
		kerF[i] *= scale;
	}
	Mat temp(1, ksize, CV_32F, &kerF[0]);
	temp.convertTo(k, CV_16S);
}

//非极大值抑制、同时得到亚像素标志位
void NMSuppression(cv::Mat& img, cv::Mat& srcX, cv::Mat& srcY, cv::Mat* Dis, cv::Mat* flg, uchar thresholdMin, uchar thresholdMax)
{
	int valMin = thresholdMin * 128 * 2;
	int valMax = thresholdMax * 128 * 2;
	*Dis = Mat(img.size(), CV_8UC1);
	*flg = Mat(img.size(), CV_8UC1);

	size_t colEnd = AlignLo(img.cols - 2, 32);
	bool hasEnd = colEnd != img.cols - 2;
	size_t Signm = img.cols - 32;
	size_t rowEnd = img.rows - 1;

	__m256i SETX = _mm256_set1_epi8(1);             //
	__m256i SETY = _mm256_set1_epi8(2);             //
	__m256i MaxTh = _mm256_set1_epi16(valMax);      //强边缘阈值
	__m256i MinTh = _mm256_set1_epi16(valMin);      //弱边缘阈值
	__m256i SET = _mm256_set1_epi8(100);            //阈值设定值大
	__m256i Zero = _mm256_set1_epi8(0);             //阈值设定值小
	__m256i OFFSet = _mm256_set1_epi16(5*128);      //边缘控制系数（增大、会增强边缘的连续性，但会产生毛刺）
	ushort* SrcPtr;
	ushort* SrcUP;
	ushort* SrcDO;
	ushort* SrcX;
	ushort* SrcY;
	uchar* DisPtr;                                  //结果图片地址
	uchar* FlgPtr;                                  //标志位结果 用于计算亚像素使用
	for (size_t i = 1; i < rowEnd; i++)
	{
		SrcUP = img.ptr<ushort>(i - 1);
		SrcPtr = img.ptr<ushort>(i);
		SrcDO = img.ptr<ushort>(i + 1);
		SrcX = srcX.ptr<ushort>(i);
		SrcY = srcY.ptr<ushort>(i);
		DisPtr = Dis->ptr<uchar>(i);
		FlgPtr = flg->ptr<uchar>(i);
		for (size_t j = 1; j < colEnd; j += 32)
		{
			__m256i MaxMaskA;
			__m256i MinMaskA;
			__m256i MaxMaskB;
			__m256i MinMaskB;

			__m256i MaskXA;
			__m256i MaskYA;
			__m256i MaskXB;
			__m256i MaskYB;

			{
				__m256i val = _mm256_loadu_epi16(SrcPtr + j);
				__m256i valX = _mm256_loadu_epi16(SrcX + j);
				__m256i valY = _mm256_loadu_epi16(SrcY + j);
				MaskXA = _mm256_cmpgt_epi16(_mm256_adds_epi16(valX, OFFSet), valY);
				MaskYA = _mm256_cmpgt_epi16(_mm256_adds_epi16(valY, OFFSet), valX);
				//分别判断xy是否满足
				__m256i YMask = _mm256_and_si256(_mm256_and_si256(_mm256_cmpgt_epi16(val, _mm256_loadu_epi16(SrcUP + j)), _mm256_cmpgt_epi16(val, _mm256_loadu_epi16(SrcDO + j))), MaskYA);
				__m256i XMask = _mm256_and_si256(_mm256_and_si256(_mm256_cmpgt_epi16(val, _mm256_loadu_epi16(SrcPtr + j + 1)), _mm256_cmpgt_epi16(val, _mm256_loadu_epi16(SrcPtr + j - 1))), MaskXA);
				//满足一个条件就认为存在数据点
				__m256i Mask = _mm256_or_si256(YMask, XMask);
				//判断强弱边缘
				MaxMaskA = _mm256_and_si256(_mm256_cmpgt_epi16(val, MaxTh), Mask);
				MinMaskA = _mm256_and_si256(_mm256_cmpgt_epi16(val, MinTh), Mask);
			}
			{
				__m256i val = _mm256_loadu_epi16(SrcPtr + j + 16);
				__m256i valX = _mm256_loadu_epi16(SrcX + j + 16);
				__m256i valY = _mm256_loadu_epi16(SrcY + j + 16);
				MaskXB = _mm256_cmpgt_epi16(_mm256_adds_epi16(valX, OFFSet), valY);
				MaskYB = _mm256_cmpgt_epi16(_mm256_adds_epi16(valY, OFFSet), valX);
				__m256i YMask = _mm256_and_si256(_mm256_and_si256(_mm256_cmpgt_epi16(val, _mm256_loadu_epi16(SrcUP + j + 16)), _mm256_cmpgt_epi16(val, _mm256_loadu_epi16(SrcDO + j + 16))), MaskYB);
				__m256i XMask = _mm256_and_si256(_mm256_and_si256(_mm256_cmpgt_epi16(val, _mm256_loadu_epi16(SrcPtr + j + 1 + 16)), _mm256_cmpgt_epi16(val, _mm256_loadu_epi16(SrcPtr + j - 1 + 16))), MaskXB);
				__m256i Mask = _mm256_or_si256(YMask, XMask);
				MaxMaskB = _mm256_and_si256(_mm256_cmpgt_epi16(val, MaxTh), Mask);
				MinMaskB = _mm256_and_si256(_mm256_cmpgt_epi16(val, MinTh), Mask);
			}
			_mm256_storeu_epi8(DisPtr + j, _mm256_adds_epu8(_mm256_blendv_epi8(Zero, SET, PackI16ToI8(MaxMaskA, MaxMaskB)), _mm256_blendv_epi8(Zero, SET, PackI16ToI8(MinMaskA, MinMaskB))));
			_mm256_storeu_epi8(FlgPtr + j, _mm256_adds_epu8(_mm256_blendv_epi8(Zero, SETX, PackI16ToI8(MaskXA, MaskXB)), _mm256_blendv_epi8(Zero, SETY, PackI16ToI8(MaskYA, MaskYB))));
		}
		if (hasEnd)
		{
			__m256i MaxMaskA;
			__m256i MinMaskA;
			__m256i MaxMaskB;
			__m256i MinMaskB;

			__m256i MaskXA;
			__m256i MaskYA;
			__m256i MaskXB;
			__m256i MaskYB;
			{
				__m256i val = _mm256_loadu_epi16(SrcPtr + Signm);
				__m256i valX = _mm256_loadu_epi16(SrcX + Signm);
				__m256i valY = _mm256_loadu_epi16(SrcY + Signm);
				MaskXA = _mm256_cmpgt_epi16(_mm256_adds_epi16(valX, OFFSet), valY);
				MaskYA = _mm256_cmpgt_epi16(_mm256_adds_epi16(valY, OFFSet), valX);
				//分别判断xy是否满足
				__m256i YMask = _mm256_and_si256(_mm256_and_si256(_mm256_cmpgt_epi16(val, _mm256_loadu_epi16(SrcUP + Signm)), _mm256_cmpgt_epi16(val, _mm256_loadu_epi16(SrcDO + Signm))), MaskYA);
				__m256i XMask = _mm256_and_si256(_mm256_and_si256(_mm256_cmpgt_epi16(val, _mm256_loadu_epi16(SrcPtr + Signm + 1)), _mm256_cmpgt_epi16(val, _mm256_loadu_epi16(SrcPtr + Signm - 1))), MaskXA);
				//满足一个条件就认为存在数据点
				__m256i Mask = _mm256_or_si256(YMask, XMask);
				//判断强弱边缘
				MaxMaskA = _mm256_and_si256(_mm256_cmpgt_epi16(val, MaxTh), Mask);
				MinMaskA = _mm256_and_si256(_mm256_cmpgt_epi16(val, MinTh), Mask);
			}
			{
				__m256i val = _mm256_loadu_epi16(SrcPtr + Signm + 16);
				__m256i valX = _mm256_loadu_epi16(SrcX + Signm + 16);
				__m256i valY = _mm256_loadu_epi16(SrcY + Signm + 16);
				MaskXB = _mm256_cmpgt_epi16(_mm256_adds_epi16(valX, OFFSet), valY);
				MaskYB = _mm256_cmpgt_epi16(_mm256_adds_epi16(valY, OFFSet), valX);
				__m256i YMask = _mm256_and_si256(_mm256_and_si256(_mm256_cmpgt_epi16(val, _mm256_loadu_epi16(SrcUP + Signm + 16)), _mm256_cmpgt_epi16(val, _mm256_loadu_epi16(SrcDO + Signm + 16))), MaskYB);
				__m256i XMask = _mm256_and_si256(_mm256_and_si256(_mm256_cmpgt_epi16(val, _mm256_loadu_epi16(SrcPtr + Signm + 1 + 16)), _mm256_cmpgt_epi16(val, _mm256_loadu_epi16(SrcPtr + Signm - 1 + 16))), MaskXB);
				__m256i Mask = _mm256_or_si256(YMask, XMask);
				MaxMaskB = _mm256_and_si256(_mm256_cmpgt_epi16(val, MaxTh), Mask);
				MinMaskB = _mm256_and_si256(_mm256_cmpgt_epi16(val, MinTh), Mask);
			}
			_mm256_storeu_epi8(DisPtr + Signm, _mm256_adds_epu8(_mm256_blendv_epi8(Zero, SET, PackI16ToI8(MaxMaskA, MaxMaskB)), _mm256_blendv_epi8(Zero, SET, PackI16ToI8(MinMaskA, MinMaskB))));
			_mm256_storeu_epi8(FlgPtr + Signm, _mm256_adds_epu8(_mm256_blendv_epi8(Zero, SETX, PackI16ToI8(MaskXA, MaskXB)), _mm256_blendv_epi8(Zero, SETY, PackI16ToI8(MaskYA, MaskYB))));

		}
	}

	//return;
	//for (size_t i = 1; i < img.rows-1; i++)
	//{
	//    for (size_t j = 1; j < img.cols-1; j++)
	//    {
	//        ushort val = img.at<ushort>(i, j);
	//        if (val> valMin)
	//        {
	//            bool flga = val > img.at<ushort>(i, j + 1) && val > img.at<ushort>(i, j - 1)&& srcX.at<ushort>(i,j)> srcY.at<ushort>(i, j);
	//            bool flgb = val > img.at<ushort>(i + 1, j) && val > img.at<ushort>(i - 1, j) && srcX.at<ushort>(i, j) < srcY.at<ushort>(i, j);
	//            if (flga || flgb)
	//            {
	//                if (val > valMax)
	//                {
	//                    Dis->at<uchar>(i, j) = 255;
	//                }
	//                else
	//                {
	//                    Dis->at<uchar>(i, j) = 100;
	//                }
	//                
	//            }
	//        }
	//    }
	//}
}

//轮廓跟踪
void GetPointList(cv::Mat& img,std::vector<std::vector<cv::Point>>& ResList)
{
	//边框部分清零、不清零存在搜索越界情况
	///或者在搜索过程中判断（影响效率）
	for (size_t i = 0; i < img.cols; i++)
	{
		img.at<uchar>(0, i) = 0;
		img.at<uchar>(1, i) = 0;
		img.at<uchar>(img.rows - 1, i) = 0;
		img.at<uchar>(img.rows - 2, i) = 0;
	}
	for (size_t i = 0; i < img.rows; i++)
	{
		img.at<uchar>(i, 0) = 0;
		img.at<uchar>(i, 1) = 0;
		img.at<uchar>(i, img.cols - 1) = 0;
		img.at<uchar>(i, img.cols - 2) = 0;
	}


	vector<Point> PointTmp(8*(img.rows+img.cols));//有内存访问错误的风险  

	//加强搜索范围 内圈+外圈
	static int directions[24][2] = { {1,0},{1,-1},{0,-1},{-1,-1},{-1,0},{-1,1},{0,1},{1,1},
		{2,0},{2,1},{2,-2},{1,-2},{0,-2},{-1,-2},{-2,-2},{-2,-1},{-2,0},{-2,1},{-2,2},{-1,2},{0,2},{1,2},{2,2} };
	int CountTmp = 0;
	int beginDirection = 0;
	bool bFindBoardpoint = false;//寻找到邻域的边界点的判定
	bool StopFind = false;
	vector<Point> PList;

	unsigned char* Ptr = img.data;

	Point startPoint(0, 0);
	Point currentPoint(0, 0);
	int addfg = 0;
	int x = 2;
	int prev = 0;
	for (int y = 2; y < img.rows - 2; y += 1)
	{
		Ptr = img.data + img.step * y;
		for (x = 2; x < img.cols - 2; x += 1)
		{
			for (; x <= img.cols - 34; x += 32)
			{
				if (_mm256_movemask_epi8(_mm256_loadu_epi8(Ptr + x)))
				{
					break;
				}
			}
			if (Ptr[x] > 199)//起始点必须是强边缘
			{
				startPoint.x = x;
				startPoint.y = y;
				currentPoint = startPoint;
				Ptr[x] = 0;
				CountTmp = 0;
				StopFind = false;
				PointTmp[CountTmp].x = x;
				PointTmp[CountTmp].y = y;
				CountTmp++;
				bool backoff = true;//后退标志位、当找到终点的时候后退一步继续寻找
				beginDirection = 0;
				int count = 0;
				//切入目标点 正方向寻找
				while (!StopFind)
				{					
					while (true)
					{
						if (beginDirection > 23)
						{
							if (backoff && CountTmp>2)//必须找到两个以上的点才可以后退
							{
								currentPoint = PointTmp[CountTmp - 3];
								beginDirection = 0;
								backoff = false;
							}
							else
							{
								StopFind = true;
								break;
							}
						}
						count = beginDirection<0? beginDirection+8: beginDirection;
						unsigned char* Pmr = (unsigned char*)(img.data + (currentPoint.y + directions[count][1]) * img.step + currentPoint.x + directions[count][0]);
						if (*Pmr > 99)//连接点可以是弱边缘和强边缘
						{

							backoff = true;
							currentPoint.x += directions[count][0];
							currentPoint.y += directions[count][1];
							//在当前位置后退两个点位
							beginDirection = beginDirection > 7 ? (beginDirection - 8)/2 : beginDirection<-6?0: beginDirection;
							beginDirection =  beginDirection - 2;

							PointTmp[CountTmp].x = currentPoint.x;
							PointTmp[CountTmp].y = currentPoint.y;
							CountTmp++;
							*Pmr = 11;
							break;
						}
						beginDirection++;
					}

				}
				if (CountTmp > 0)
				{
					PList.insert(PList.end(), PointTmp.begin(), PointTmp.begin() + CountTmp);
					reverse(PList.begin(), PList.end());
				}

				CountTmp = 0;
				StopFind = false;
				backoff = true;
				currentPoint = startPoint;
				beginDirection = 0;
				//反方向寻找
				while (!StopFind)
				{				
					while (true)
					{
						if (beginDirection > 23)
						{
							if (backoff && CountTmp > 2)//必须找到两个以上的点才可以后退
							{
								currentPoint = PointTmp[CountTmp - 3];
								beginDirection = 0;
								backoff = false;
							}
							else
							{
								StopFind = true;
								break;
							}
						}
						count = beginDirection < 0 ? beginDirection + 8 : beginDirection;
						unsigned char* Pmr = (unsigned char*)(img.data + (currentPoint.y + directions[count][1]) * img.step + currentPoint.x + directions[count][0]);
						if (*Pmr >99)
						{
							backoff = true;
							currentPoint.x += directions[count][0];
							currentPoint.y += directions[count][1];

							beginDirection = beginDirection > 7 ? (beginDirection - 8) / 2 : beginDirection < -6 ? 0: beginDirection;
							beginDirection = beginDirection - 2;

							PointTmp[CountTmp].x = currentPoint.x;
							PointTmp[CountTmp].y = currentPoint.y;
							CountTmp++;
							*Pmr = 10;
							break;
						}
						beginDirection++;
					}

				}
				if (CountTmp > 0)
				{
					PList.insert(PList.end(), PointTmp.begin(), PointTmp.begin() + CountTmp);

				}
				if (PList.size() > 5)
				{
					ResList.push_back(PList);
				}
				PList.clear();
			}
		}
	}

}

void EdgeSubpix(cv::Mat& img, double ThreadMin, double ThreadMax, std::vector<std::vector<cv::Point2f>>& RList, double sigma)
{
	if (img.rows < 5 || img.cols < 5)
	{
		return;
	}
	//高斯滤波、平滑图像
	///提高图像的连贯性、超大半径平滑时，用于提取干扰大的轮廓。
	Mat Gaus;
	GaussianBlur(img, Gaus, Size(0, 0), sigma, sigma);
	//创建核心
	cv::Mat Dx, Dy, dist;
	Mat K;
	getCannyKernel(K, sigma*0.7);
	//提取梯度 #非常关键  常规Sobel无法提取平滑后的梯度 或提取的梯度非常弱。
	///采用可变核心、根据平滑程度动态卷积。可提取恒定强度的核心
	Filter2d16(Gaus, K, &Dx, &Dy);
	//叠加梯度，直接相加。
	GenDistance(Dx, Dy, &dist);
	//非极大值抑制，提取轮廓点
	Mat  Plist, Flgs;
	NMSuppression(dist, Dx, Dy, &Plist, &Flgs, ThreadMin, ThreadMax);
	//轮廓跟踪、将轮廓连接到一起（耗时严重、没想出来优化方案）
	std::vector<std::vector<cv::Point>> ResList;
	GetPointList(Plist, ResList);

	cv::Mat Gx, Gy;
	Gx = Mat(img.size(),CV_32FC1);
	Gy = Mat(img.size(), CV_32FC1);
	//提取亚像素结果
	///或者经过筛选、预处理后再计算亚像素。提高效率
	RList.clear();
	for (auto items: ResList)
	{
		if (items.size()>500)
		{
			vector<cv::Point2f> tmp;
			tmp.reserve(items.size());
			for (auto item : items)
			{
				//uchar val = Flgs.at<uchar>(item.y, item.x);
				//int tmpval = val == 3 ? 1 : 0;
				//int Dx = val == 1 ? 1 : tmpval;
				//int Dy = val == 2 ? 1 : tmpval;
				//double a = dist.at<ushort>(item.y - Dy, item.x - Dx);
				//double b = dist.at<ushort>(item.y, item.x);
				//double c = dist.at<ushort>(item.y + Dy, item.x + Dx);
				//double tmpv = (a - b - b + c);
				//double offset = 0.5 * (a - c) / tmpv;

				//Point2f p = (Point2f)item + Point2f(offset * Dx, offset * Dy);
				//if (!isnan(p.x) && !isinf(p.x))
				//{
				//	tmp.push_back(p);
				//}

				tmp.push_back(Point2f(item.x, item.y));
			}
			RList.push_back(tmp);
		}
	}
}

